/* ITU-T G.729 Software Package Release 2 (November 2006) */
/* version 1.3 modified september 1999 */
/* G.729 with ANNEX D   Version 1.1    Last modified: March 1998 */

#include "typedef.h"
#include "basic_op.h"
#include "oper_32b.h"
#include "ld8k.h"
#include "ld8kd.h"
#include "tab_ld8k.h"
#include "tabld8kd.h"

static void Gbk_presel(
   Word16 best_gain[],     /* (i) [0] Q9 : unquantized pitch gain     */
                           /* (i) [1] Q2 : unquantized code gain      */
   Word16 *cand1,          /* (o)    : index of best 1st stage vector */
   Word16 *cand2,          /* (o)    : index of best 2nd stage vector */
   Word16 gcode0           /* (i) Q4 : presearch for gain codebook    */
);
/*---------------------------------------------------------------------------*
 * Function  Qua_gain                                                        *
 * ~~~~~~~~~~~~~~~~~~                                                        *
 * Inputs:                                                                   *
 *   code[]     :Innovative codebook.                                        *
 *   g_coeff[]  :Correlations compute for pitch.                             *
 *   L_subfr    :Subframe length.                                            *
 *                                                                           *
 * Outputs:                                                                  *
 *   gain_pit   :Quantized pitch gain.                                       *
 *   gain_cod   :Quantized code gain.                                        *
 *                                                                           *
 * Return:                                                                   *
 *   Index of quantization.                                                  *
 *                                                                           *
 *--------------------------------------------------------------------------*/
Word16 Qua_gain_6k(
   Word16 code[],       /* (i) Q13 :Innovative vector.             */
   Word16 g_coeff[],    /* (i)     :Correlations <xn y1> -2<y1 y1> */
                        /*            <y2,y2>, -2<xn,y2>, 2<y1,y2> */
   Word16 exp_coeff[],  /* (i)     :Q-Format g_coeff[]             */
   Word16 L_subfr,      /* (i)     :Subframe length.               */
   Word16 *gain_pit,    /* (o) Q14 :Pitch gain.                    */
   Word16 *gain_cod,    /* (o) Q1  :Code gain.                     */
   Word16 tameflag,      /* (i)     : set to 1 if taming is needed  */
   Word16 past_qua_en[]
)
{
   Word16  i, j, index1, index2;
   Word16  cand1, cand2;
   Word16  exp, gcode0, exp_gcode0, gcode0_org, e_min ;
   Word16  nume, denom, inv_denom;
   Word16  exp1,exp2,exp_nume,exp_denom,exp_inv_denom,sft,tmp;
   Word16  g_pitch, g2_pitch, g_code, g2_code, g_pit_cod;
   Word16  coeff[5], coeff_lsf[5];
   Word16  exp_min[5];
   Word32  L_gbk12;
   Word32  L_tmp, L_dist_min, L_temp, L_tmp1, L_tmp2, L_acc, L_accb;
   Word16  best_gain[2];

  /*---------------------------------------------------*
   *-  energy due to innovation                       -*
   *-  predicted energy                               -*
   *-  predicted codebook gain => gcode0[exp_gcode0]  -*
   *---------------------------------------------------*/
   Gain_predict(past_qua_en, code, L_subfr, &gcode0, &exp_gcode0);

  /*-----------------------------------------------------------------*
   *  pre-selection                                                  *
   *-----------------------------------------------------------------*/
 /*-----------------------------------------------------------------*
   *  calculate best gain                                            *
   *                                                                 *
   *  tmp = -1./(4.*coeff[0]*coeff[2]-coeff[4]*coeff[4]) ;           *
   *  best_gain[0] = (2.*coeff[2]*coeff[1]-coeff[3]*coeff[4])*tmp ;  *
   *  best_gain[1] = (2.*coeff[0]*coeff[3]-coeff[1]*coeff[4])*tmp ;  *
   *  gbk_presel(best_gain,&cand1,&cand2,gcode0) ;                   *
   *                                                                 *
   *-----------------------------------------------------------------*/
  /*-----------------------------------------------------------------*
   *  tmp = -1./(4.*coeff[0]*coeff[2]-coeff[4]*coeff[4]) ;           *
   *-----------------------------------------------------------------*/
   L_tmp1 = L_mult( g_coeff[0], g_coeff[2] );
   exp1   = add( add( exp_coeff[0], exp_coeff[2] ), 1-2 );
   L_tmp2 = L_mult( g_coeff[4], g_coeff[4] );
   exp2   = add( add( exp_coeff[4], exp_coeff[4] ), 1 );

   if( sub(exp1, exp2)>0 ){
      L_tmp = L_sub( L_shr( L_tmp1, sub(exp1,exp2) ), L_tmp2 );
      exp = exp2;
   }
   else{
      L_tmp = L_sub( L_tmp1, L_shr( L_tmp2, sub(exp2,exp1) ) );
      exp = exp1;
   }
   sft = norm_l( L_tmp );
   denom = extract_h( L_shl(L_tmp, sft) );
   exp_denom = sub( add( exp, sft ), 16 );

   inv_denom = div_s(16384,denom);
   inv_denom = negate( inv_denom );
   exp_inv_denom = sub( 14+15, exp_denom );

  /*-----------------------------------------------------------------*
   *  best_gain[0] = (2.*coeff[2]*coeff[1]-coeff[3]*coeff[4])*tmp ;  *
   *-----------------------------------------------------------------*/
   L_tmp1 = L_mult( g_coeff[2], g_coeff[1] );
   exp1   = add( exp_coeff[2], exp_coeff[1] );
   L_tmp2 = L_mult( g_coeff[3], g_coeff[4] );
   exp2   = add( add( exp_coeff[3], exp_coeff[4] ), 1 );

   if( sub(exp1, exp2)>0 ){
      L_tmp = L_sub( L_shr( L_tmp1, add(sub(exp1,exp2),1 )), L_shr( L_tmp2,1 ) );
      exp = sub(exp2,1);
   }
   else{
      L_tmp = L_sub( L_shr( L_tmp1,1 ), L_shr( L_tmp2, add(sub(exp2,exp1),1 )) );
      exp = sub(exp1,1);
   }
   sft = norm_l( L_tmp );
   nume = extract_h( L_shl(L_tmp, sft) );
   exp_nume = sub( add( exp, sft ), 16 );

   sft = sub( add( exp_nume, exp_inv_denom ), (9+16-1) );
   L_acc = L_shr( L_mult( nume,inv_denom ), sft );

   best_gain[0] = extract_h( L_acc );             /*-- best_gain[0]:Q9 --*/
   if (tameflag == 1){
     if(sub(best_gain[0], GPCLIP2) > 0) best_gain[0] = GPCLIP2;
   }

  /*-----------------------------------------------------------------*
   *  best_gain[1] = (2.*coeff[0]*coeff[3]-coeff[1]*coeff[4])*tmp ;  *
   *-----------------------------------------------------------------*/
   L_tmp1 = L_mult( g_coeff[0], g_coeff[3] );
   exp1   = add( exp_coeff[0], exp_coeff[3] ) ;
   L_tmp2 = L_mult( g_coeff[1], g_coeff[4] );
   exp2   = add( add( exp_coeff[1], exp_coeff[4] ), 1 );

   if( sub(exp1, exp2)>0 ){
      L_tmp = L_sub( L_shr( L_tmp1, add(sub(exp1,exp2),1) ), L_shr( L_tmp2,1 ) );
      exp = sub(exp2,1);
   }
   else{
      L_tmp = L_sub( L_shr( L_tmp1,1 ), L_shr( L_tmp2, add(sub(exp2,exp1),1) ) );
      exp = sub(exp1,1);
   }
   sft = norm_l( L_tmp );
   nume = extract_h( L_shl(L_tmp, sft) );
   exp_nume = sub( add( exp, sft ), 16 );
   sft = sub( add( exp_nume, exp_inv_denom ), (2+16-1) );
   L_acc = L_shr( L_mult( nume,inv_denom ), sft );
   best_gain[1] = extract_h( L_acc );             /*-- best_gain[1]:Q2 --*/

   /*--- Change Q-format of gcode0 ( Q[exp_gcode0] -> Q4 ) ---*/
   if( sub(exp_gcode0,4) >= 0 ){
      gcode0_org = shr( gcode0, sub(exp_gcode0,4) );
   }
   else{
      L_acc = L_deposit_l( gcode0 );
      L_acc = L_shl( L_acc, sub( (4+16), exp_gcode0 ) );
      gcode0_org = extract_h( L_acc );              /*-- gcode0_org:Q4 --*/
   }

  /*----------------------------------------------*
   *   - presearch for gain codebook -            *
   *----------------------------------------------*/
   Gbk_presel(best_gain, &cand1, &cand2, gcode0_org );

    /*---------------------------------------------------------------------------*
     *                                                                           *
     * Find the best quantizer.                                                  *
     *                                                                           *
     *  dist_min = MAX_32;                                                       *
     *  for ( i=0 ; i<NCAN1 ; i++ ){                                             *
     *    for ( j=0 ; j<NCAN2 ; j++ ){                                           *
     *      g_pitch = gbk1[cand1+i][0] + gbk2[cand2+j][0];                       *
     *      g_code = gcode0 * (gbk1[cand1+i][1] + gbk2[cand2+j][1]);             *
     *      dist = g_pitch*g_pitch * coeff[0]                                    *
     *           + g_pitch         * coeff[1]                                    *
     *           + g_code*g_code   * coeff[2]                                    *
     *           + g_code          * coeff[3]                                    *
     *           + g_pitch*g_code  * coeff[4] ;                                  *
     *                                                                           *
     *      if (dist < dist_min){                                                *
     *        dist_min = dist;                                                   *
     *        indice1 = cand1 + i ;                                              *
     *        indice2 = cand2 + j ;                                              *
     *      }                                                                    *
     *    }                                                                      *
     *  }                                                                        *
     *                                                                           *
     * g_pitch         = Q13                                                     *
     * g_pitch*g_pitch = Q11:(13+13+1-16)                                        *
     * g_code          = Q[exp_gcode0-3]:(exp_gcode0+(13-1)+1-16)                *
     * g_code*g_code   = Q[2*exp_gcode0-21]:(exp_gcode0-3+exp_gcode0-3+1-16)     *
     * g_pitch*g_code  = Q[exp_gcode0-5]:(13+exp_gcode0-3+1-16)                  *
     *                                                                           *
     * term 0: g_pitch*g_pitch*coeff[0] ;exp_min0 = 13             +exp_coeff[0] *
     * term 1: g_pitch        *coeff[1] ;exp_min1 = 14             +exp_coeff[1] *
     * term 2: g_code*g_code  *coeff[2] ;exp_min2 = 2*exp_gcode0-19+exp_coeff[2] *
     * term 3: g_code         *coeff[3] ;exp_min3 = exp_gcode0  - 2+exp_coeff[3] *
     * term 4: g_pitch*g_code *coeff[4] ;exp_min4 = exp_gcode0  - 3+exp_coeff[4] *
     *                                                                           *
     *---------------------------------------------------------------------------*/
       exp_min[0] = add( exp_coeff[0], 13 );
       exp_min[1] = add( exp_coeff[1], 14 );
       exp_min[2] = add( exp_coeff[2], sub( shl( exp_gcode0, 1 ), 19 ) );
       exp_min[3] = add( exp_coeff[3], sub( exp_gcode0, 2 ) );
       exp_min[4] = add( exp_coeff[4], sub( exp_gcode0, 3 ) );

       e_min = exp_min[0];
       for(i=1; i<5; i++){
          if( sub(exp_min[i], e_min) < 0 ){
             e_min = exp_min[i];
          }
       }

       /* align coeff[] and save in special 32 bit double precision */
       for(i=0; i<5; i++){
         j = sub( exp_min[i], e_min );
         L_tmp = L_deposit_h( g_coeff[i] );
         L_tmp = L_shr( L_tmp, j );          /* L_tmp:Q[exp_g_coeff[i]+16-j] */
         L_Extract( L_tmp, &coeff[i], &coeff_lsf[i] );          /* DPF */
       }

       /* Codebook search */
       L_dist_min = MAX_32;
       /* initialization used only to suppress Microsoft Visual C++  warnings */
       index1 = cand1;
       index2 = cand2;

        if(tameflag == 1){
           for(i=0; i<NCAN1_6K; i++){
              for(j=0; j<NCAN2_6K; j++){
                 g_pitch = add( gbk1_6k[cand1+i][0], gbk2_6k[cand2+j][0] );  /* Q14 */
                 if(g_pitch < GP0999) {
                 L_acc = L_deposit_l( gbk1_6k[cand1+i][1] );
                 L_accb = L_deposit_l( gbk2_6k[cand2+j][1] );               /* Q14 */
                 L_tmp = L_add( L_acc,L_accb );
                 tmp = extract_l( L_shr( L_tmp,1 ) );                       /* Q13 */

                 g_code   = mult( gcode0, tmp );         /*  Q[exp_gcode0+13-15] */
                 g2_pitch = mult(g_pitch, g_pitch);                       /* Q13 */
                 g2_code  = mult(g_code,  g_code);       /* Q[2*exp_gcode0-4-15] */
                 g_pit_cod= mult(g_code,  g_pitch);      /* Q[exp_gcode0-2+14-15] */

                 L_tmp = Mpy_32_16(coeff[0], coeff_lsf[0], g2_pitch);
                 L_tmp = L_add(L_tmp, Mpy_32_16(coeff[1], coeff_lsf[1], g_pitch) );
                 L_tmp = L_add(L_tmp, Mpy_32_16(coeff[2], coeff_lsf[2], g2_code) );
                 L_tmp = L_add(L_tmp, Mpy_32_16(coeff[3], coeff_lsf[3], g_code) );
                 L_tmp = L_add(L_tmp, Mpy_32_16(coeff[4], coeff_lsf[4], g_pit_cod) );

                 L_temp = L_sub(L_tmp, L_dist_min);
                 if( L_temp < 0L ){
                    L_dist_min = L_tmp;
                    index1 = add(cand1,i);
                    index2 = add(cand2,j);
                 }
                }
              }
           }

        }

        else{
           for(i=0; i<NCAN1_6K; i++){
              for(j=0; j<NCAN2_6K; j++){
                 g_pitch = add( gbk1_6k[cand1+i][0], gbk2_6k[cand2+j][0] );  /* Q14 */
                 L_acc = L_deposit_l( gbk1_6k[cand1+i][1] );
                 L_accb = L_deposit_l( gbk2_6k[cand2+j][1] );               /* Q14 */
                 L_tmp = L_add( L_acc,L_accb );
                 tmp = extract_l( L_shr( L_tmp,1 ) );                       /* Q13 */

                 g_code   = mult( gcode0, tmp );         /*  Q[exp_gcode0+13-15] */
                 g2_pitch = mult(g_pitch, g_pitch);                       /* Q13 */
                 g2_code  = mult(g_code,  g_code);       /* Q[2*exp_gcode0-4-15] */
                 g_pit_cod= mult(g_code,  g_pitch);      /* Q[exp_gcode0-2+14-15] */

                 L_tmp = Mpy_32_16(coeff[0], coeff_lsf[0], g2_pitch);
                 L_tmp = L_add(L_tmp, Mpy_32_16(coeff[1], coeff_lsf[1], g_pitch) );
                 L_tmp = L_add(L_tmp, Mpy_32_16(coeff[2], coeff_lsf[2], g2_code) );
                 L_tmp = L_add(L_tmp, Mpy_32_16(coeff[3], coeff_lsf[3], g_code) );
                 L_tmp = L_add(L_tmp, Mpy_32_16(coeff[4], coeff_lsf[4], g_pit_cod) );

                 L_temp = L_sub(L_tmp, L_dist_min);
                 if( L_temp < 0L ){
                    L_dist_min = L_tmp;
                    index1 = add(cand1,i);
                    index2 = add(cand2,j);
                 }
              }
           }
        }
           /* Read the quantized gain */
          /*-----------------------------------------------------------------*
           * *gain_pit = gbk1[indice1][0] + gbk2[indice2][0];                *
           *-----------------------------------------------------------------*/
           *gain_pit = add( gbk1_6k[index1][0], gbk2_6k[index2][0] );      /* Q14 */
          /*-----------------------------------------------------------------*
           * *gain_code = (gbk1[indice1][1]+gbk2[indice2][1]) * gcode0;      *
           *-----------------------------------------------------------------*/
           L_acc = L_deposit_l( gbk1_6k[index1][1] );
           L_accb = L_deposit_l( gbk2_6k[index2][1] );
           L_gbk12 = L_add( L_acc, L_accb );                          /* Q14 */
           L_gbk12 = L_shr(L_gbk12, 1);                               /* Q13 */
           tmp = extract_l( L_gbk12 );                                /* Q13 */
           L_acc = L_mult(tmp, gcode0);                /* Q[exp_gcode0+13+1] */

           L_acc = L_shl(L_acc, add( negate(exp_gcode0),(-13-1+1+16) ));
           *gain_cod = extract_h( L_acc );                             /* Q1 */

    /*----------------------------------------------*
    * update table of past quantized energies      *
    *----------------------------------------------*/
    Gain_update( past_qua_en, L_gbk12 );

   return( add( (Word16)(map1_6k[index1]*(Word16)NCODE2_6K), map2_6k[index2] ) );

}
        /*---------------------------------------------------------------------------*
         * Function Gbk_presel                                                       *
         * ~~~~~~~~~~~~~~~~~~~                                                       *
         *   - presearch for gain codebook -                                         *
     *---------------------------------------------------------------------------*/

static void Gbk_presel(
       Word16 best_gain[],     /* (i) [0] Q9 : unquantized pitch gain     */
                               /* (i) [1] Q2 : unquantized code gain      */
       Word16 *cand1,          /* (o)    : index of best 1st stage vector */
       Word16 *cand2,          /* (o)    : index of best 2nd stage vector */
       Word16 gcode0           /* (i) Q4 : presearch for gain codebook    */
    )
{
   Word16    acc_h;
   Word16    sft_x,sft_y;
   Word32    L_acc,L_preg,L_cfbg,L_tmp,L_tmp_x,L_tmp_y;
   Word32 L_temp;

 /*--------------------------------------------------------------------------*
   x = (best_gain[1]-(coef[0][0]*best_gain[0]+coef[1][1])*gcode0) * inv_coef;
  *--------------------------------------------------------------------------*/
   L_cfbg = L_mult( coef_6k[0][0], best_gain[0] );     /* L_cfbg:Q19 -> !!y */
   L_acc = L_shr( L_coef_6k[1][1], 15 );               /* L_acc:Q19     */
   L_acc = L_add( L_cfbg , L_acc );
   acc_h = extract_h( L_acc );                         /* acc_h:Q3      */
   L_preg = L_mult( acc_h, gcode0 );                   /* L_preg:Q8     */
   L_acc = L_shl( L_deposit_l( best_gain[1] ), 6 );    /* L_acc:Q8      */
   L_acc = L_sub( L_acc, L_preg );
   acc_h = extract_h( L_shl( L_acc,2 ) );              /* L_acc_h:Q[-6] */
   L_tmp_x = L_mult( acc_h, INV_COEF_6K );             /* L_tmp_x:Q15   */

 /*--------------------------------------------------------------------------*
   y = (coef[1][0]*(-coef[0][1]+best_gain[0]*coef[0][0])*gcode0
                                      -coef[0][0]*best_gain[1]) * inv_coef;
  *--------------------------------------------------------------------------*/
   L_acc = L_shr( L_coef_6k[0][1], 10 );               /* L_acc:Q19   */
   L_acc = L_sub( L_cfbg, L_acc );                     /* !!x -> L_cfbg:Q19 */
   acc_h = extract_h( L_acc );                         /* acc_h:Q3    */
   acc_h = mult( acc_h, gcode0 );                      /* acc_h:Q[-8] */
   L_tmp = L_mult( acc_h, coef_6k[1][0] );             /* L_tmp:Q9    */

   L_preg = L_mult( coef_6k[0][0], best_gain[1] );     /* L_preg:Q12  */
   L_acc = L_sub( L_tmp, L_shr(L_preg,3) );            /* L_acc:Q9    */

   acc_h = extract_h( L_shl( L_acc,2 ) );              /* acc_h:Q[-5] */
   L_tmp_y = L_mult( acc_h, INV_COEF_6K );             /* L_tmp_y:Q16 */

   sft_y = (13+4+1)-16;         /* (Q[thr1_6k]+Q[gcode0]+1)-Q[L_tmp_y] */
   sft_x = (15+4+1)-15;         /* (Q[thr2_6k]+Q[gcode0]+1)-Q[L_tmp_x] */

   if(gcode0>0){
      /*-- pre select codebook #1 --*/
      *cand1 = 0 ;
      do{
         L_temp = L_sub( L_tmp_y, L_shr(L_mult(thr1_6k[*cand1],gcode0),sft_y));
         if(L_temp >0L  ){
        (*cand1) =add(*cand1,1);
     }
    else               break ;
      } while(sub((*cand1),(NCODE1_6K-NCAN1_6K))<0) ;
      /*-- pre select codebook #2 --*/
      *cand2 = 0 ;
      do{
        L_temp = L_sub( L_tmp_x , L_shr(L_mult(thr2_6k[*cand2],gcode0),sft_x));
         if( L_temp >0L) {
        (*cand2) =add(*cand2,1);
     }
         else               break ;
      } while(sub((*cand2),(NCODE2_6K-NCAN2_6K))<0) ;
   }
   else{
      /*-- pre select codebook #1 --*/
      *cand1 = 0 ;
      do{
        L_temp = L_sub(L_tmp_y ,L_shr(L_mult(thr1_6k[*cand1],gcode0),sft_y));
         if( L_temp <0L){
        (*cand1) =add(*cand1,1);
     }
         else               break ;
      } while(sub((*cand1),(NCODE1_6K-NCAN1_6K))) ;
      /*-- pre select codebook #2 --*/
      *cand2 = 0 ;
      do{
         L_temp =L_sub(L_tmp_x ,L_shr(L_mult(thr2_6k[*cand2],gcode0),sft_x));
         if( L_temp <0L){
        (*cand2) =add(*cand2,1);
     }
         else               break ;
      } while(sub( (*cand2),(NCODE2_6K-NCAN2_6K))) ;
   }

   return ;
}

